
/**
 * @file SolverPPPUCAR.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//  shjzhang
//
//  2020/01/16
//  remove stateMap/covMap, which makes the program complicated
//  shjzhang.
//  
//  2020/02/09
//  modify from SolverPPP for UC-PPP
//
//============================================================================


#include "SolverPPPUCAR.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "PartialAR.hpp"
#include "MatrixFunctors.hpp"


#define debug 1
#define detail 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SolverPPPUCAR::classIndex = 9600000;


      // Returns an index identifying this object.
   int SolverPPPUCAR::getIndex() const
   { return index; }


      // Returns a string identifying this object.
   std::string SolverPPPUCAR::getClassName() const
   { return "SolverPPPUCAR"; }

   void SolverPPPUCAR::Init(void)
   {
   }

   void SolverPPPUCAR::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
          // These variables are, by default, SourceID-indexed
       Variable dx( TypeID::dStaX );
       Variable dy( TypeID::dStaY );
       Variable dz( TypeID::dStaZ );
       dx.setInitialVariance(100*100);
       dy.setInitialVariance(100*100);
       dz.setInitialVariance(100*100);


       Variable dn( TypeID::dStaLat );
       Variable de( TypeID::dStaLon );
       Variable du( TypeID::dStaH );
       dn.setInitialVariance(100*100);
       de.setInitialVariance(100*100);
       du.setInitialVariance(100*100);

       whiteNoiseDXModel.setSigma(100);
       whiteNoiseDYModel.setSigma(100);
       whiteNoiseDZModel.setSigma(100);

       whiteNoiseDNModel.setSigma(100);
       whiteNoiseDEModel.setSigma(100);
       whiteNoiseDUModel.setSigma(100);

       if(kinematic)
       {
           dx.setModel(&whiteNoiseDXModel);
           dy.setModel(&whiteNoiseDYModel);
           dz.setModel(&whiteNoiseDZModel);
           dn.setModel(&whiteNoiseDNModel);
           de.setModel(&whiteNoiseDEModel);
           du.setModel(&whiteNoiseDUModel);
       }
       else
       {
           dx.setModel(&constantDXModel);
           dy.setModel(&constantDXModel);
           dz.setModel(&constantDXModel);
           dn.setModel(&constantDNModel);
           de.setModel(&constantDEModel);
           du.setModel(&constantDUModel);
       }
       
       VariableSet neuVarSet,xyzVarSet;

       xyzVarSet.insert(dx);
       xyzVarSet.insert(dy);
       xyzVarSet.insert(dz);

       neuVarSet.insert(dn);
       neuVarSet.insert(de);
       neuVarSet.insert(du);

       Variable tropo( TypeID::wetMap,  &tropoModel );

       Variable cdt( TypeID::dcdtSta,    &cdtModel);
       Variable cdtGAL( TypeID::dcdtStaGAL, &cdtGALModel);
       Variable cdtBDS( TypeID::dcdtStaBDS, &cdtBDSModel);

       Variable cdtL1GPS(TypeID::dcdtStaL1GPS, &cdtL1GPSModel);
       Variable cdtL2GPS(TypeID::dcdtStaL2GPS, &cdtL2GPSModel);
       Variable cdtL5GPS(TypeID::dcdtStaL5GPS, &cdtL5GPSModel);

       Variable cdtL1GAL(TypeID::dcdtStaL1GAL, &cdtL1GALModel);
       Variable cdtL5GAL(TypeID::dcdtStaL5GAL, &cdtL5GALModel);
       Variable cdtL7GAL(TypeID::dcdtStaL7GAL, &cdtL7GALModel);
       Variable cdtL8GAL(TypeID::dcdtStaL8GAL, &cdtL8GALModel);
       Variable cdtL6GAL(TypeID::dcdtStaL6GAL, &cdtL6GALModel);

       Variable cdtL2BDS(TypeID::dcdtStaL2BDS, &cdtL2BDSModel);
       Variable cdtL7BDS(TypeID::dcdtStaL7BDS, &cdtL7BDSModel);
       Variable cdtL6BDS(TypeID::dcdtStaL6BDS, &cdtL6BDSModel);

       // source-indexed and sat-indexed 
       Variable iono( TypeID::ionoL1, &ionoModel, true, true, false);
       
       // SourceID-indexed, SatID-indexed, arc-indexed
       Variable GPSBL1( TypeID::BL1, &GPSBL1Model, true, true, true );
       Variable GPSBL2( TypeID::BL2, &GPSBL2Model, true, true, true );
       Variable GPSBL5( TypeID::BL5, &GPSBL5Model, true, true, true );

       Variable GALBL1( TypeID::BL1, &GALBL1Model, true, true, true);
       Variable GALBL5( TypeID::BL5, &GALBL5Model, true, true, true);
       Variable GALBL7( TypeID::BL7, &GALBL7Model, true, true, true);
       Variable GALBL8( TypeID::BL8, &GALBL8Model, true, true, true);
       Variable GALBL6( TypeID::BL6, &GALBL6Model, true, true, true);

       Variable BDSBL2( TypeID::BL2, &BDSBL2Model, true, true, true);
       Variable BDSBL7( TypeID::BL7, &BDSBL7Model, true, true, true);
       Variable BDSBL6( TypeID::BL6, &BDSBL6Model, true, true, true);

       //
       // Define Equation object for code/phase observations, and add variables 
       //

       // GPS PC
       Equation equC1GPS( TypeID::prefitC1 );
       Equation equC2GPS( TypeID::prefitC2 );
       Equation equC5GPS( TypeID::prefitC5 );

       // GPS LC
       Equation equL1GPS( TypeID::prefitL1 );
       Equation equL2GPS( TypeID::prefitL2 );
       Equation equL5GPS( TypeID::prefitL5 );

       // GAL PC
       Equation equC1GAL( TypeID::prefitC1, SatelliteSystem::Galileo );
       Equation equC5GAL( TypeID::prefitC5, SatelliteSystem::Galileo );
       Equation equC7GAL( TypeID::prefitC7, SatelliteSystem::Galileo  );
       Equation equC8GAL( TypeID::prefitC8, SatelliteSystem::Galileo  );
       Equation equC6GAL( TypeID::prefitC6, SatelliteSystem::Galileo );

       // GAL LC
       Equation equL1GAL( TypeID::prefitL1, SatelliteSystem::Galileo );
       Equation equL5GAL( TypeID::prefitL5, SatelliteSystem::Galileo );
       Equation equL7GAL( TypeID::prefitL7, SatelliteSystem::Galileo );
       Equation equL8GAL( TypeID::prefitL8, SatelliteSystem::Galileo );
       Equation equL6GAL( TypeID::prefitL6, SatelliteSystem::Galileo );

       // BDS-2 PC
       Equation equC2BDS( TypeID::prefitC7, SatelliteSystem::BDS );
       Equation equC7BDS( TypeID::prefitC7, SatelliteSystem::BDS );
       Equation equC6BDS( TypeID::prefitC6, SatelliteSystem::BDS );

       // BDS-2 LC                                        
       Equation equL2BDS( TypeID::prefitL2, SatelliteSystem::BDS );
       Equation equL7BDS( TypeID::prefitL7, SatelliteSystem::BDS );
       Equation equL8BDS( TypeID::prefitL8, SatelliteSystem::BDS );
       Equation equL6BDS( TypeID::prefitL6, SatelliteSystem::BDS );


       //////////////////////////
       // insert variables now
       //////////////////////////
       if(useNEU) 
       { 
            equC1GPS.addVarSet(neuVarSet);
            equC2GPS.addVarSet(neuVarSet);
            equC5GPS.addVarSet(neuVarSet);

            equL1GPS.addVarSet(neuVarSet);
            equL2GPS.addVarSet(neuVarSet);
            equL5GPS.addVarSet(neuVarSet);

            equC1GAL.addVarSet(neuVarSet);
            equC5GAL.addVarSet(neuVarSet);
            equC7GAL.addVarSet(neuVarSet);
            equC8GAL.addVarSet(neuVarSet);
            equC6GAL.addVarSet(neuVarSet);

            equL1GAL.addVarSet(neuVarSet);
            equL5GAL.addVarSet(neuVarSet);
            equL7GAL.addVarSet(neuVarSet);
            equL8GAL.addVarSet(neuVarSet);
            equL6GAL.addVarSet(neuVarSet);

            equC2BDS.addVarSet(neuVarSet);
            equC7BDS.addVarSet(neuVarSet);
            equC6BDS.addVarSet(neuVarSet);


            equL2BDS.addVarSet(neuVarSet);
            equL7BDS.addVarSet(neuVarSet);
            equL6BDS.addVarSet(neuVarSet);

       }
       else       
       {   
            equC1GPS.addVarSet(xyzVarSet);
            equC2GPS.addVarSet(xyzVarSet);
            equC5GPS.addVarSet(xyzVarSet);

            equL1GPS.addVarSet(xyzVarSet);
            equL2GPS.addVarSet(xyzVarSet);
            equL5GPS.addVarSet(xyzVarSet);

            equC1GAL.addVarSet(xyzVarSet);
            equC5GAL.addVarSet(xyzVarSet);
            equC7GAL.addVarSet(xyzVarSet);
            equC8GAL.addVarSet(xyzVarSet);
            equC6GAL.addVarSet(xyzVarSet);

            equL1GAL.addVarSet(xyzVarSet);
            equL5GAL.addVarSet(xyzVarSet);
            equL7GAL.addVarSet(xyzVarSet);
            equL8GAL.addVarSet(xyzVarSet);
            equL6GAL.addVarSet(xyzVarSet);

            equC2BDS.addVarSet(xyzVarSet);
            equC7BDS.addVarSet(xyzVarSet);
            equC6BDS.addVarSet(xyzVarSet);

            equL2BDS.addVarSet(xyzVarSet);
            equL7BDS.addVarSet(xyzVarSet);
            equL6BDS.addVarSet(xyzVarSet);

       }

       //
       // trop
       //

       equC1GPS.addVariable(tropo);
       equC2GPS.addVariable(tropo);
       equC5GPS.addVariable(tropo);

       equL1GPS.addVariable(tropo);
       equL2GPS.addVariable(tropo);
       equL5GPS.addVariable(tropo);

       equC1GAL.addVariable(tropo);
       equC5GAL.addVariable(tropo);
       equC7GAL.addVariable(tropo);
       equC8GAL.addVariable(tropo);
       equC6GAL.addVariable(tropo);

       equL1GAL.addVariable(tropo);
       equL5GAL.addVariable(tropo);
       equL7GAL.addVariable(tropo);
       equL8GAL.addVariable(tropo);
       equL6GAL.addVariable(tropo);

       equC2BDS.addVariable(tropo);
       equC7BDS.addVariable(tropo);
       equC6BDS.addVariable(tropo);


       equL2BDS.addVariable(tropo);
       equL7BDS.addVariable(tropo);
       equL6BDS.addVariable(tropo);

       // cdt for GPS/GAL/BDS
       equC1GPS.addVariable(cdt, 1.0);
       equC2GPS.addVariable(cdt, 1.0);
       equC5GPS.addVariable(cdt, 1.0);

       equC1GAL.addVariable(cdtGAL, 1.0);
       equC5GAL.addVariable(cdtGAL, 1.0);
       equC7GAL.addVariable(cdtGAL, 1.0);
       equC8GAL.addVariable(cdtGAL, 1.0);
       equC6GAL.addVariable(cdtGAL, 1.0);

       equC2BDS.addVariable(cdtBDS, 1.0);
       equC7BDS.addVariable(cdtBDS, 1.0);
       equC6BDS.addVariable(cdtBDS, 1.0);

////////////////////
//     equC1BDS.addVariable(cdtBDS, 1.0);
//     equC5BDS.addVariable(cdtBDS, 1.0);
//     equC8BDS.addVariable(cdtBDS, 1.0);
////////////////////

       // warning:::
       // carrier-phase phase-clock bias 
       equL1GPS.addVariable(cdtL1GPS, 1.0);
       equL2GPS.addVariable(cdtL2GPS, 1.0);
       equL5GPS.addVariable(cdtL5GPS, 1.0);

       equL1GAL.addVariable(cdtL1GAL, 1.0);
       equL5GAL.addVariable(cdtL5GAL, 1.0);
       equL7GAL.addVariable(cdtL7GAL, 1.0);
       equL8GAL.addVariable(cdtL8GAL, 1.0);
       equL6GAL.addVariable(cdtL6GAL, 1.0);

       equL2BDS.addVariable(cdtL2BDS, 1.0);
       equL7BDS.addVariable(cdtL7BDS, 1.0);
       equL6BDS.addVariable(cdtL6BDS, 1.0);


       // iono
       equC1GPS.addVariable(iono,  1.0);
       equC2GPS.addVariable(iono,  GAMMA_GPS_L1L2);
       equC5GPS.addVariable(iono,  GAMMA_GPS_L1L5);

       equL1GPS.addVariable(iono,  -1.0);
       equL2GPS.addVariable(iono,  -GAMMA_GPS_L1L2);
       equL5GPS.addVariable(iono,  -GAMMA_GPS_L1L5);

       equC1GAL.addVariable(iono,  1.0);
       equC5GAL.addVariable(iono,  GAMMA_GAL_L1L5);
       equC7GAL.addVariable(iono,  GAMMA_GAL_L1L7);
       equC8GAL.addVariable(iono,  GAMMA_GAL_L1L8);
       equC6GAL.addVariable(iono,  GAMMA_GAL_L1L6);

       equL1GAL.addVariable(iono,  -1.0);
       equL5GAL.addVariable(iono,  -GAMMA_GAL_L1L5);
       equL7GAL.addVariable(iono,  -GAMMA_GAL_L1L7);
       equL8GAL.addVariable(iono,  -GAMMA_GAL_L1L8);
       equL6GAL.addVariable(iono,  -GAMMA_GAL_L1L6);

       equC2BDS.addVariable(iono,  GAMMA_BDS_L1L2);
       equC7BDS.addVariable(iono,  GAMMA_BDS_L1L7);
       equC6BDS.addVariable(iono,  GAMMA_BDS_L1L6);

       equL2BDS.addVariable(iono,  -GAMMA_BDS_L1L2);
       equL7BDS.addVariable(iono,  -GAMMA_BDS_L1L7);
       equL6BDS.addVariable(iono,  -GAMMA_BDS_L1L6);

//     equC1BDS.addVariable(iono,  1.0);
//     equC5BDS.addVariable(iono,  GAMMA_BDS_L1L5);
//     equC8BDS.addVariable(iono,  GAMMA_BDS_L1L8);

//     equL1BDS.addVariable(iono,  -1.0);
//     equL5BDS.addVariable(iono,  -GAMMA_BDS_L1L5);
//     equL8BDS.addVariable(iono,  -GAMMA_BDS_L1L8);

       // ambiguity 

       equL1GPS.addVariable(GPSBL1,  L1_WAVELENGTH_GPS);
       equL2GPS.addVariable(GPSBL2,  L2_WAVELENGTH_GPS);
       equL5GPS.addVariable(GPSBL5,  L5_WAVELENGTH_GPS);

       equL1GAL.addVariable(GALBL1,  L1_WAVELENGTH_GAL);
       equL5GAL.addVariable(GALBL5,  L5_WAVELENGTH_GAL);
       equL7GAL.addVariable(GALBL7,  L7_WAVELENGTH_GAL);
       equL8GAL.addVariable(GALBL8,  L8_WAVELENGTH_GAL);
       equL6GAL.addVariable(GALBL6,  L6_WAVELENGTH_GAL);

       equL2BDS.addVariable(BDSBL2,  L2_WAVELENGTH_BDS);
       equL7BDS.addVariable(BDSBL7,  L7_WAVELENGTH_BDS);
       equL6BDS.addVariable(BDSBL6,  L6_WAVELENGTH_BDS);


       // define the weight for all combinations relative PC12GPS

       equC1GPS.setWeight(1.0/0.3*1.0/0.3);  // 1.0/0.3*1.0/0.3
       equC2GPS.setWeight(1.0/0.3*1.0/0.3);
       equC5GPS.setWeight(1.0/0.3*1.0/0.3);

       equC1GAL.setWeight(1.0/0.3*1.0/0.3);
       equC5GAL.setWeight(1.0/0.3*1.0/0.3);
       equC7GAL.setWeight(1.0/0.3*1.0/0.3);
       equC8GAL.setWeight(1.0/0.3*1.0/0.3);
       equC6GAL.setWeight(1.0/0.3*1.0/0.3);

       equC2BDS.setWeight(1.0/0.3*1.0/0.3);
       equC7BDS.setWeight(1.0/0.3*1.0/0.3);
       equC6BDS.setWeight(1.0/0.3*1.0/0.3);

/////////////////////////////////////////////
//     equC1BDS.setWeight(1.0/0.3*1.0/0.3);
//     equC5BDS.setWeight(1.0/0.3*1.0/0.3);
//     equC8BDS.setWeight(1.0/0.3*1.0/0.3);
/////////////////////////////////////////////

       equL1GPS.setWeight(1.0/0.003*1.0/0.003);
       equL2GPS.setWeight(1.0/0.003*1.0/0.003);
       equL5GPS.setWeight(1.0/0.003*1.0/0.003);

       equL1GAL.setWeight(1.0/0.003*1.0/0.003);
       equL5GAL.setWeight(1.0/0.003*1.0/0.003);
       equL7GAL.setWeight(1.0/0.003*1.0/0.003);
       equL8GAL.setWeight(1.0/0.003*1.0/0.003);
       equL6GAL.setWeight(1.0/0.003*1.0/0.003);

       equL2BDS.setWeight(1.0/0.003*1.0/0.003);
       equL7BDS.setWeight(1.0/0.003*1.0/0.003);
       equL6BDS.setWeight(1.0/0.003*1.0/0.003);

//////////////////////////////////////////////////
//     equL1BDS.setWeight(1.0/0.003*1.0/0.003);
//     equL5BDS.setWeight(1.0/0.003*1.0/0.003);
//     equL8BDS.setWeight(1.0/0.003*1.0/0.003);
//////////////////////////////////////////////////

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(usingGPS)
       {
           for(TypeIDSet::const_iterator typeIt=gpsTypeSet.begin();
                   typeIt!=gpsTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::L1 )
               {
                   equSystem.addEquation(equC1GPS);
                   equSystem.addEquation(equL1GPS);
               }
               else if ( (*typeIt) == TypeID::L2 )
               {
                   equSystem.addEquation(equC2GPS);
                   equSystem.addEquation(equL2GPS);
               }
               else if ( (*typeIt) == TypeID::L5 )
               {
                   equSystem.addEquation(equC5GPS);
                   equSystem.addEquation(equL5GPS);
               }
           }
       }

       if(usingGAL)
       {
           for(TypeIDSet::const_iterator typeIt=galTypeSet.begin();
               typeIt!=galTypeSet.end();
               typeIt++)
           {
               if( (*typeIt) == TypeID::L1 )
               {
                   equSystem.addEquation(equC1GAL);
                   equSystem.addEquation(equL1GAL);
               }
               else if ( (*typeIt) == TypeID::L5 )
               {
                   equSystem.addEquation(equC5GAL);
                   equSystem.addEquation(equL5GAL);
               }
               else if ( (*typeIt) == TypeID::L7 )
               {
                   equSystem.addEquation(equC7GAL);
                   equSystem.addEquation(equL7GAL);
               }
               else if ( (*typeIt) == TypeID::L8 )
               {
                   equSystem.addEquation(equC8GAL);
                   equSystem.addEquation(equL8GAL);
               }
               else if ( (*typeIt) == TypeID::L6 )
               {
                   equSystem.addEquation(equC6GAL);
                   equSystem.addEquation(equL6GAL);
               }
           }

       }

       if(usingBDS)
       {
           for(TypeIDSet::const_iterator typeIt=bdsTypeSet.begin();
               typeIt!=bdsTypeSet.end();
               typeIt++)
           {
               if ( (*typeIt) == TypeID::L2 )
               {
                   equSystem.addEquation(equC2BDS);
                   equSystem.addEquation(equL2BDS);
               }
               else if ( (*typeIt) == TypeID::L7 )
               {
                   equSystem.addEquation(equC7BDS);
                   equSystem.addEquation(equL7BDS);
               }
               else if ( (*typeIt) == TypeID::L6 )
               {
                   equSystem.addEquation(equC6BDS);
                   equSystem.addEquation(equL6BDS);
               }
           }
       }

   } // end of setUpEquations




      /* Returns a reference to a gnssRinex object after solving
       *  the previously defined equation system.
       *
       * @param gData    Data object holding the data.
       */
   void SolverPPPUCAR::Process(commonHeader& comHeader, gnssRinex& gData)
       noexcept(false)
   {

      try
      {
         preCompute( gData );
         Compute( gData );
         postCompute( gData );
      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );
         GPSTK_THROW(e);
      }

      return;

   }  // End of method 'SolverPPPUCAR::Prepare()'



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverPPPUCAR::preCompute( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
         if(debug)
         {
             cout << gRin.header.epoch << endl;
             cout << "begin of SolverPPPUCAR:" << gRin.header.source << endl;
             std::list<Equation> descripEquList;
             descripEquList = equSystem.getDescripEqus();
             cout << "size:" << descripEquList.size() << endl;
             for(std::list<Equation>::iterator it=descripEquList.begin();
                     it!=descripEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }
         }

            // Prepare the equation system with current data
         equSystem.Prepare(gRin);


            // Get the number of unknowns being processed
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         currentUnkSet =  equSystem.getVarUnknowns() ;

         if(debug)
         {
             std::list<Equation> currentEquList;
             currentEquList = equSystem.getCurrentEquationsList();
             for(std::list<Equation>::iterator it=currentEquList.begin();
                     it!=currentEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }

             cout << "currentUnkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=currentUnkSet.begin();
                     vIt!=currentUnkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }

         // reset filter
         CommonTime epoch( gRin.header.epoch );

            // Feed the filter with the correct state and covariance matrix
         if( firstTime )
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            for( auto v: currentUnkSet )
            {
                int index = v.getNowIndex();
                initialCov(index,index) = v.getInitialVariance();
            }

               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

               // No longer first time
            firstTime = false;
         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            for( auto v: currentUnkSet )
            {
                int now_index = v.getNowIndex();
                int old_index = v.getPreIndex();
                if( old_index != -1)
                {
                    currentState(now_index) = solution( old_index );
                }
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang
               // modify the code, 2020/01/16, shjzhang

               // We need a copy of 'currentUnkSet'
            VariableSet tempSet(currentUnkSet);
            for( auto v: currentUnkSet ) 
            {
                int nowIndex = v.getNowIndex();
                int oldIndex = v.getPreIndex();

                // old Variable
                if( oldIndex != -1 )
                {
                    for( auto v2: tempSet )
                    {
                        int nowIndex2 = v2.getNowIndex();
                        int oldIndex2 = v2.getPreIndex();

                        // old Variable
                        if( oldIndex2 != -1 )
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = covMatrix(oldIndex, oldIndex2);
                        }
                        else // new variable
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = 0.0;
                        }
                    }
                }
                else // new variable
                {
                    // only diagnol element is assigned to the new variable
                    currentCov(nowIndex, nowIndex) = v.getInitialVariance();
                }

                // erase var
                tempSet.erase(v);

            }  // End of for( VariableSet::const_iterator itVar1 = currentUnkSet...'

               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;

         }  // End of 'if(firstTime)'

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

         if(detail)
         {
            cout << "phiMatrix" << endl;
            cout <<  phiMatrix  << endl;

            cout << "qMatrix" << endl;
            cout <<  qMatrix  << endl;
         }

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverPPPUCAR::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   gnssRinex& SolverPPPUCAR::Compute( gnssRinex& gRin )
       noexcept(false)
   {
      TimeUpdate( phiMatrix, qMatrix );
      Constraint();
      MeasUpdate( measVector, hMatrix, rMatrix );

         // Return  
      return gRin;

   }  // End of method 'SolverPPPUCAR::Compute()'


      // Predict the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverPPPUCAR::TimeUpdate( const Matrix<double>& phiMatrix,
                                 const Matrix<double>& processNoiseCovariance )
       noexcept(false)
   {

         // Check whether the phiMatrix is square
      if (!(phiMatrix.isSquare()))
      {
         InvalidSolver e("phiMatrix is not square");
         GPSTK_THROW(e);
      }

         // Get the number of unknowns being processed
      int numUnknowns( equSystem.getTotalNumVariables() );
      int stateSize(xhat.size());
      if(!(stateSize==numUnknowns))
      {
         InvalidSolver e("TimeUpdate(): Size of a posteriori state estimation vector \
do not match the number of unknowns");
         GPSTK_THROW(e);
      }

      int phiRow = static_cast<int>(phiMatrix.rows());
      if (!(phiRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of phiMatrix");
         GPSTK_THROW(e);
      }

      if (!(qMatrix.isSquare()))
      {
         InvalidSolver e("qMatrix is not square");
         GPSTK_THROW(e);
      }

      int qRow = static_cast<int>(qMatrix.rows());
      if (!(qRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of qMatrix");
         GPSTK_THROW(e);
      }

      try
      {
            // Compute the a priori state vector
         xhatminus = phiMatrix*xhat;
         Matrix<double> phiT(transpose(phiMatrix));
            // Compute the a priori estimate error covariance matrix
         Pminus = phiMatrix*P*phiT + processNoiseCovariance;

      }
      catch(...)
      {
         InvalidSolver e("TimeUpdate(): Unable to predict next state.");
         GPSTK_THROW(e);
         return -1;
      }
        
      return 0;

   }  // End of method 'SolverPPPUCAR::Compute()'

   void SolverPPPUCAR::Constraint()
   {

          // Get matrices and vectors out of equation system
          // Measurements vector (Prefit-residuals)
       measVector = equSystem.getPrefitsVector();
       hMatrix = equSystem.getGeometryMatrix();
       rMatrix = equSystem.getWeightsMatrix();

       /////////////////
       // Ambiguity constraint
       /////////////////

       VariableDataMap datumAmbData;
       //
       // BL1 datum
       //
       if(usingGPS)
       {
            // firstly, extract ambiguities from solution/covMatrix
            SatIDSet satSetL1;
            SatIDSet satSetL2;

            satSetL1 = extractSatSet(TypeID::BL1, SatelliteSystem::GPS);
            satSetL2 = extractSatSet(TypeID::BL2, SatelliteSystem::GPS);

            // now, let's get commonSatSet for L1/L2
            SatIDSet commonSatSet12;
            set_intersection(satSetL1.begin(), satSetL1.end(), 
                             satSetL2.begin(), satSetL2.end(), 
                             inserter(commonSatSet12, commonSatSet12.begin()) );

            if(detail)
            {
                cout << "satSetL1" << endl;
                for(auto s: satSetL1)
                {
                    cout << s << endl;
                }

                cout << "satSetL2" << endl;
                for(auto s: satSetL2)
                {
                    cout << s << endl;
                }
            }

            if(detail)
            {
                cout << "commonSatSet12" << endl;
                for(auto s: commonSatSet12)
                {
                    cout << s << endl;
                }
            }

            // fixing BWL12 
            if(commonSatSet12.size() == 0)
            {
                cerr << "SolverPPPUCAR::commonSatSet12 is zero" << endl;
                exit(-1);
            }

            VariableSet BL1Set = extractAmbSet(TypeID::BL1,SatelliteSystem::GPS);
            VariableSet BL2Set = extractAmbSet(TypeID::BL2,SatelliteSystem::GPS);

            if(detail)
            {
                cout << "BL1Set" << endl;
                for(auto v: BL1Set)
                {
                    cout << v << endl;
                }
                cout << "BL2Set" << endl;
                for(auto v: BL2Set)
                {
                    cout << v << endl;
                }
            }

            VariableSet BWL12Set;
            composedAmbsMap composedAmbData;
            for(auto sat:commonSatSet12)
            {
                // temporary variable
                Variable BWL12(TypeID::BWL12);
                BWL12.setSatellite(sat);
                BWL12.setArc(0);

                BWL12Set.insert(BWL12);
                for(auto amb:BL1Set)
                {
                    if(amb.getSatellite() == sat)
                    {
                        composedAmbData[BWL12].amb1 = amb ;
                        composedAmbData[BWL12].coeff1 = 1.0;
                    }
                }

                for(auto amb:BL2Set)
                {
                    if(amb.getSatellite() == sat)
                    {
                        composedAmbData[BWL12].amb2 = amb;
                        composedAmbData[BWL12].coeff2 = -1.0;
                    }
                }
            }

            if(debug)
            {
                cout << "BWL12Set" << endl;
                for(auto a: BWL12Set )
                {
                    cout << a << endl;
                }
            }


            // compute widelane ambiguities
            VariableDataMap   BWLData;
            CovarianceDataMap BWLCovData;
            getCombinedAmbs(xhatminus, Pminus, 
                            BWL12Set, composedAmbData, BWLData, BWLCovData);
            if(debug)
            {
                cout << "BWLData" << endl;
                for(auto vd: BWLData)
                {
                    cout << vd.first << ":" << vd.second << endl;
                }
            }

            // now, let's get the WideLane ambiguity datum

            AmbDatum ambDatum;
            VariableDataMap NWL12Data = ambDatum.Prepare(BWLData, BWLCovData);

            if(NWL12Data.size() != 1)
            {
                cerr << "NWL datum is not correct!" << endl;
                exit(-1);
            }

            cout << "NWL12Data datum" << endl;
            for(auto vd: NWL12Data)
            {
                cout << vd.first << ":" << vd.second << endl;
            }

            // now, let's fix the L1 ambiguities with the widelane ambiguities
            // are fixed already.
            auto ambIter = NWL12Data.begin();

            Variable datumBWL12 = ambIter->first; 
            double NWL12 = ambIter->second;

            Variable datumBL1 = composedAmbData[datumBWL12].amb1;
            Variable datumBL2 = composedAmbData[datumBWL12].amb2;

            double BL1 = xhatminus(datumBL1.getNowIndex());
            double BL2 = xhatminus(datumBL2.getNowIndex());

            //////////////////////////////////////////
            // now, let's compute the ionosphere-free interger ambiguity combination
            // BLC = BL1  + f2/(f1-f2)(BL1-BL2)
            //////////////////////////////////////////

            double f1 = L1_FREQ_GPS;
            double f2 = L2_FREQ_GPS;

            // ionosphere-free ambiguity BLC12 with wavelength 10cm
            double BLC12 = BL1 + f2/(f1-f2)*(BL1-BL2);

            // now, let's correct BLC12 with NWL12
            double NL1 = BLC12 - f2/(f1-f2)*NWL12;  

            cout << "NL1:" << endl;
            cout << NL1 << endl;

            // Let's get the integer of NLC12;
            double NL1Fixed = std::floor(NL1 + 0.5);
            double NL2Fixed = NL1Fixed - NWL12;

            //////////////////
            // NL1 = (NCL12 - 60*NWL12)/17
            // NL2 = (NCL12 - 77*NWL12)/17
            //////////////////
            datumAmbData[datumBL1] = NL1Fixed;
            datumAmbData[datumBL2] = NL2Fixed;

       }

       if(usingGAL)
       {
           cerr << "SolverPPPUCAR::not support now" << endl;
           exit(-1);
       }

       if(usingBDS)
       {
           cerr << "SolverPPPUCAR::not support now" << endl;
           exit(-1);
       }

       // datumAmbData
       if(debug)
       {
           cout << "datumAmbData" << endl;
           for(auto dda: datumAmbData)
           {
               cout << dda.first << ":" << dda.second << endl;
           }
       }

       int numEqu = measVector.size();
       int numUnknowns = currentUnkSet.size();
       int numEquALL = ( numEqu + datumAmbData.size() ) ;

       Vector<double> tempPrefit;
       Matrix<double> tempGeometry, tempWeight;

       tempPrefit.resize(numEquALL, 0.0);
       tempGeometry.resize(numEquALL, numUnknowns, 0.0);
       tempWeight.resize(numEquALL, numEquALL, 0.0);

       for(int i=0; i<numEqu; i++)
       {
             // Prefit
          tempPrefit(i) = measVector(i);
             // Geometry
          for(int j=0; j<numUnknowns; j++)
          {
             tempGeometry(i,j) = hMatrix(i,j);
          }
             // Weight
          tempWeight(i,i) = rMatrix(i,i);
       }

       int numRow = numEqu;
       for(auto vd: datumAmbData)
       {
           int numCol = vd.first.getNowIndex();
           tempPrefit(numRow) = vd.second;
           tempGeometry(numRow, numCol) = 1.0;
           tempWeight(numRow, numRow) = 1.0E+9;
           numRow++;
       }

       measVector.resize(numEquALL, 0.0);
       hMatrix.resize(numEquALL, numUnknowns, 0.0);
       rMatrix.resize(numEquALL, numEquALL, 0.0);

       measVector = tempPrefit;
       hMatrix = tempGeometry;
       rMatrix = tempWeight;

       if(detail)
       {
           cout << "after constraint" << endl;
           cout << ios::fixed << setprecision(3) << "measVector" << endl;
           cout << ios::fixed << setprecision(3) << measVector << endl;
           cout << ios::fixed << setprecision(3) << "hMatrix" << endl;
           cout << ios::fixed << setprecision(3) << hMatrix << endl;
           cout << ios::fixed << setprecision(3) << "rMatrix" << endl;
           cout << ios::fixed << setprecision(3) << rMatrix << endl;
       }

   }


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverPPPUCAR::MeasUpdate( const Vector<double>& prefitResiduals,
                                 const Matrix<double>& designMatrix,
                                 const Matrix<double>& weightMatrix )
       noexcept(false)
   {

         // By default, results are invalid
      valid = false;
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("MeasUpdate(): Weight matrix is not square");
         GPSTK_THROW(e);
      }


      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match dimension of \
weightMatrix");
         GPSTK_THROW(e);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match \
dimension of designMatrix");
         GPSTK_THROW(e);
      }


      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }


      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }


         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> designMatrixT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         Matrix<double> invTemp( designMatrixT * weightMatrix * designMatrix +
                                 invPMinus );
            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
         xhat = P * ( (designMatrixT * weightMatrix * prefitResiduals) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;
         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);
         // If everything is fine so far, then the results should be valid
      valid = true;

      return 0;

   }  // End of method 'SolverPPPUCAR::MeasUpdate()'



      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverPPPUCAR::postCompute( gnssRinex& gRin )
       noexcept(false)
   {
      try
      {

         //
         // Save needed gnss rinex data for backward-processing
         //

         CommonTime epoch(gRin.header.epoch);

         if(debug)
         {
             cout << YDSTime(epoch) << endl;
             cout << "solution:" << endl;
             cout << solution << endl;
         }

         // Insert forward gnssRinex to forwardRinexData
         gnssRinex gBack(gRin);

         TypeIDSet typeSet;
         if(useNEU)
         {
            typeSet.insert(TypeID::dStaLat);
            typeSet.insert(TypeID::dStaLon);
            typeSet.insert(TypeID::dStaH);
         }
         else
         {
            typeSet.insert(TypeID::dStaX);
            typeSet.insert(TypeID::dStaY);
            typeSet.insert(TypeID::dStaZ);
         }
         typeSet.insert(TypeID::wetMap);
         typeSet.insert(TypeID::satArc);
         typeSet.insert(TypeID::weight);

         if(usingGPS)
         {
             for( TypeIDSet::iterator typeIt = gpsTypeSet.begin();
                  typeIt != gpsTypeSet.end();
                  typeIt++ )
             {
                 if( (*typeIt) == TypeID::L1 )
                 {
                    typeSet.insert(TypeID::prefitC1);
                    typeSet.insert(TypeID::prefitL1);
                 }
                 else if( (*typeIt) == TypeID::L2 )
                 {
                    typeSet.insert(TypeID::prefitC2);
                    typeSet.insert(TypeID::prefitL2);
                 }
                 else if( (*typeIt) == TypeID::L5 )
                 {
                    typeSet.insert(TypeID::prefitC5);
                    typeSet.insert(TypeID::prefitL5);
                 }
             }
         }

         if(usingGAL)
         {
             for( TypeIDSet::iterator typeIt = galTypeSet.begin();
                  typeIt != galTypeSet.end();
                  typeIt++ )
             {
                 if( (*typeIt) == TypeID::L1 )
                 {
                    typeSet.insert(TypeID::prefitC1);
                    typeSet.insert(TypeID::prefitL1);
                 }
                 else if( (*typeIt) == TypeID::L5 )
                 {
                    typeSet.insert(TypeID::prefitC5);
                    typeSet.insert(TypeID::prefitL5);
                 }
                 else if( (*typeIt) == TypeID::L7 )
                 {
                    typeSet.insert(TypeID::prefitC7);
                    typeSet.insert(TypeID::prefitL7);
                 }
                 else if( (*typeIt) == TypeID::L8 )
                 {
                    typeSet.insert(TypeID::prefitC8);
                    typeSet.insert(TypeID::prefitL8);
                 }
                 else if( (*typeIt) == TypeID::L6 )
                 {
                    typeSet.insert(TypeID::prefitC6);
                    typeSet.insert(TypeID::prefitL6);
                 }
             }
         }

         if(usingBDS)
         {
             for( TypeIDSet::iterator typeIt = bdsTypeSet.begin();
                  typeIt != bdsTypeSet.end();
                  typeIt++ )
             {
                 if( (*typeIt) == TypeID::L6 )
                 {
                    typeSet.insert(TypeID::prefitC6);
                    typeSet.insert(TypeID::prefitL6);
                 }
                 else if( (*typeIt) == TypeID::L7 )
                 {
                    typeSet.insert(TypeID::prefitC7);
                    typeSet.insert(TypeID::prefitL7);
                 }
             }
         }

         // keep needed observation data for backward processing
         gBack.keepOnlyTypeID(typeSet);

            // save forward-processing data
         if(forwardProcessing)
         {

             // let's store the nominal initial position from BasicModel
             nominalPosData[epoch] = gRin.header.srcPos;

             if(debug)
             {
                for(auto v : currentUnkSet)
                {
                    if( v.getArcIndexed() )
                    {
                        cout << "PPP ambs:" << v << endl;
                    }
                }
             }

             int numAmb(0);
             for(auto v : currentUnkSet)
             {
                 if( v.getArcIndexed() )
                 {
                     numAmb++;
                 }
             }

             int numSat = gRin.numSats();

             Vector<double> stateFixed;
             Matrix<double> covFixed;
             int numFixed;
             double ratio;

             // sequential partial ambiguity resolution
             seqAR(solution, covMatrix, stateFixed, covFixed, numFixed, ratio);

             if(pFixedSolStreamFwd!=NULL)
                printSols(pFixedSolStreamFwd, epoch, solution, stateFixed, 
                          numSat, numFixed, ratio);

             // store the forward rinex data
             forwardRinexData[epoch] = gBack;
             forwardStateData[epoch] = solution;
             forwardCovData[epoch] = covMatrix;
         }


      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverPPPUCAR::postCompute()'

   void SolverPPPUCAR::PostProcess(commonHeader& comHeader)
       noexcept(false)
   {
       try
       {
           // now, backward processing
           forwardProcessing=false;


           // loop all saved data in forward processing
           for(epochGnssRinexMap::reverse_iterator reverseIt = forwardRinexData.rbegin();
               reverseIt != forwardRinexData.rend();
               ++reverseIt)
           {
               CommonTime epoch( (*reverseIt).first );

               gnssRinex gRin( (*reverseIt).second );

               // Backward filtering
               Process(gRin);

               // Get forward state/cov from forwardStateData and forwardCovData
               Vector<double> lastStateVec;
               Matrix<double> lastCovMatrix;

               lastStateVec = forwardStateData[epoch];
               lastCovMatrix = forwardCovData[epoch];

               Vector<double> smoothedStateVec;
               Matrix<double> smoothedCovMatrix;


               int numAmb(0);
               for(auto v : currentUnkSet)
               {
                   if( v.getArcIndexed() )
                   {
                       numAmb++;
                   }
               }

               int numSat = gRin.numSats();

               // smoothe the forward and backward state and covariance
               smoothSolution(lastStateVec, lastCovMatrix, 
                              smoothedStateVec, smoothedCovMatrix);

               // print ambiguities 
               if(pAmbStreamSmoothed!=NULL)
                   printAmbs(pAmbStreamSmoothed, epoch, smoothedStateVec, smoothedCovMatrix);

               // Ambiguity-Fixing and update sols

               Vector<double> stateFixed;
               Matrix<double> covFixed;
               int numFixed;
               double ratio;
  
  
               // sequential-partial ambiguity resolution
               seqAR(smoothedStateVec, smoothedCovMatrix, 
                     stateFixed, covFixed, numFixed, ratio);
  
  

               if(pFixedSolStreamSmoothed!=NULL)
                   printSols(pFixedSolStreamSmoothed, epoch, 
                             smoothedStateVec, stateFixed, 
                             numAmb, numFixed, ratio);
  
           } // end of 'for(epoch)'


           // at last free the random
           forwardRinexData.clear();
           forwardStateData.clear();
           forwardCovData.clear();

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );

          GPSTK_THROW(e);

       }

       return;
   }

   void SolverPPPUCAR::smoothSolution( Vector<double>& oldStateVec, 
                                   Matrix<double>& oldCovMatrix,
                                   Vector<double>& smoothedStateVec, 
                                   Matrix<double>& smoothedCovMatrix)
   {

       try
       {
           if(solution.size() != oldStateVec.size())
           {
              InvalidSolver e("smoothSolution(): size of stateMap don't match size of the oldState");
              GPSTK_THROW(e);
           }

           Matrix<double> currentWeight;
           Matrix<double> oldWeight;

           try
           {
               currentWeight = inverseChol(covMatrix);
               oldWeight = inverseChol(oldCovMatrix);
           }
           catch(Exception& u)
           {
               cerr << "SolverPPPUCAR::smoothSolution: covMatrix inversion error!" << endl;
               exit(-1);
           }

           smoothedCovMatrix = inverseChol( currentWeight +  oldWeight);
           smoothedStateVec = smoothedCovMatrix * ( currentWeight * solution+ oldWeight *oldStateVec);

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );
          GPSTK_THROW(e);
       }

   };


   int SolverPPPUCAR::seqAR(Vector<double>& state, 
                            Matrix<double>& covMatrix,
                            Vector<double>& stateFixed, 
                            Matrix<double>& covFixed,
                            int& numFixedL1,
                            double& ratioL1)
   {

       // firstly, extract ambiguities from solution/covMatrix
       SatIDSet satSetL1;
       SatIDSet satSetL2;

       satSetL1 = extractSatSet(TypeID::BL1, SatelliteSystem::GPS);
       satSetL2 = extractSatSet(TypeID::BL2, SatelliteSystem::GPS);

       VariableSet BL1Set = extractAmbSet(TypeID::BL1,SatelliteSystem::GPS);
       VariableSet BL2Set = extractAmbSet(TypeID::BL2,SatelliteSystem::GPS);

       // now, let's get commonSatSet for L1/L2
       SatIDSet commonSatSet12;
       set_intersection(satSetL1.begin(), satSetL1.end(), 
                        satSetL2.begin(), satSetL2.end(), 
                        inserter(commonSatSet12, commonSatSet12.begin()) );


       if(commonSatSet12.size() == 0)
       {
           cerr << "SolverPPPUCAR: commonSatSet12.size is zero" << endl;
           exit(-1);
       }

       VariableSet BWL12Set;
       composedAmbsMap  composedAmbData;
       for(auto sat:commonSatSet12)
       {
           // temporary variable
           Variable BWL12(TypeID::BWL12);
           BWL12.setSatellite(sat);

           // BWL12Set
           BWL12Set.insert(BWL12);

           // now, let's store the uc-ambiguity the BWL is composed of 
           for(auto amb:BL1Set)
           {
               if(amb.getSatellite() == sat)
               {
                   composedAmbData[BWL12].amb1 = amb ;
                   composedAmbData[BWL12].coeff1 = 1.0;
               }
           }

           for(auto amb:BL2Set)
           {
               if(amb.getSatellite() == sat)
               {
                   composedAmbData[BWL12].amb2 = amb;
                   composedAmbData[BWL12].coeff2 = -1.0;
               }
           }
       }

       if(debug)
       {
           cout << "BWL12Set" << endl;
           for(auto a: BWL12Set )
           {
               cout << a << endl;
           }
       }

       // compute widelane ambiguities
       VariableDataMap   BWLData;
       CovarianceDataMap BWLCovData;
       getCombinedAmbs(solution, covMatrix, 
                       BWL12Set, composedAmbData, BWLData, BWLCovData);

       // Ambiguity fixing using lambda
       VariableDataMap fixedAmbWL12;
       PartialAR parSolver;
       parSolver.Round(BWLData, BWLCovData, fixedAmbWL12);

       cout << "float BWL:" << BWLData.size() << endl;
       for(auto nd: BWLData)
       {
           cout << nd.first << ": value:" 
                << nd.second << " var:" << BWLCovData[nd.first][nd.first] << endl;
       }

       cout << "fixed BWL:" << fixedAmbWL12.size() << endl;
       for(auto nd: fixedAmbWL12)
       {
           cout << nd.first << ":" << nd.second << endl;
       }

       // Update solutions with fixed ambiguities
       Vector<double> stateFixedWL;
       Matrix<double> covMatrixFixedWL;
       updateSolsForCombAmbs(fixedAmbWL12, 
                             composedAmbData,
                             state, covMatrix, 
                             stateFixedWL, covMatrixFixedWL);

       BL1Set.clear();
       for(auto ad: fixedAmbWL12)
       {
           Variable BL1 = composedAmbData[ad.first].amb1;
           BL1Set.insert( BL1 );
       }

       // get L1 ambs from alrealdy fixed state/covMatrix
       VariableDataMap BL1Data;
       CovarianceDataMap BL1CovData;
       getL1Ambs( stateFixedWL,
                  covMatrixFixedWL,
                  BL1Set,
                  BL1Data, 
                  BL1CovData );

       // BL1Data
       if(debug)
       {
          cout << "BL1Data" << endl;
          for(auto ad: BL1Data)
          {
              cout << ad.first << ":" << ad.second << endl;
          }
       }

       // fix L1 ambiguities
       VariableDataMap fixedBL1Data;
       double ratioBL1 = parSolver.PAR(BL1Data, BL1CovData, fixedBL1Data);

       cout << "ratioBL1" << endl;
       cout << ratioBL1 << endl; 

       // numFixedL1
       int numFixedBL1 = fixedBL1Data.size();

       cout << "numFixedBL1" << endl;
       cout <<  numFixedBL1  << endl;
       for(auto ad: fixedBL1Data)
       {
           cout << ad.first << ":" << ad.second << endl;
       }



       Vector<double> stateFixedL1;
       Matrix<double> covFixedL1;
       updateSols(fixedBL1Data, 
                  stateFixedWL, covMatrixFixedWL, 
                  stateFixedL1, covFixedL1);

       // return 
       stateFixed = stateFixedL1;
       covFixed = covFixedL1;

       numFixedL1 = numFixedBL1;
       ratioL1 = ratioBL1;

   };

   // getCombinedAmbs
   void SolverPPPUCAR::getCombinedAmbs( Vector<double>&    state,
                                        Matrix<double>&    covMatrix,
                                        VariableSet&       combAmbSet,
                                        composedAmbsMap&   composedAmbData,
                                        VariableDataMap&   combAmbData, 
                                        CovarianceDataMap& combAmbCovData )
   {
       VariableSet ambUCSet;
       for(auto v: combAmbSet)
       {
           Variable amb1 = composedAmbData[v].amb1;
           Variable amb2 = composedAmbData[v].amb2;
           ambUCSet.insert(amb1);
           ambUCSet.insert(amb2);
       }


       if(detail)
       {
           cout << "ambUCSet" << endl;
           for(auto a: ambUCSet)
           {
               cout << a << endl;
           }
       }


       // the size of un-combined ambiguities
       int numUC = ambUCSet.size();

       // Let's get ambVec, ambCov for ambUCSet
       Vector<double> ambUCVec(numUC, 0.0);
       Matrix<double> ambUCCov(numUC, numUC, 0.0);

       int p=0;
       for(auto amb: ambUCSet)
       {
           ambUCVec(p) = state(amb.getNowIndex());

           int q=0;
           for(auto amb2: ambUCSet)
           {
               ambUCCov(p,q) = covMatrix(amb.getNowIndex(), amb2.getNowIndex());
               q++;
           }
           p++;
       }
       
       if(detail)
       {
           cout << "ambUCVec" << endl;
           cout << ambUCVec << endl;

           cout << "ambUCCov" << endl;
           cout << ambUCCov << endl;
       }

       int numComb = combAmbSet.size();

       // Let's get map matrix 
       Matrix<double> mapUC2Comb(numComb, numUC, 0.0);

       int i=0;
       for(auto amb: combAmbSet)
       {
           Variable amb1 = composedAmbData[amb].amb1; 
           Variable amb2 = composedAmbData[amb].amb2; 
           double coeff1 = composedAmbData[amb].coeff1;
           double coeff2 = composedAmbData[amb].coeff2;

           int m = 0;
           for(auto ucAmb: ambUCSet)
           {
               if(ucAmb == amb1 )
               {
                   mapUC2Comb(i, m) = coeff1;
               }
               if(ucAmb == amb2 )
               {
                   mapUC2Comb(i, m) = coeff2;
               }

               m++;
           }

           i++;
       }

       if(detail)
       {
       cout << "mapUC2Comb" << endl;
       cout << mapUC2Comb << endl;
       }

       // now, compute ambVec/ambCov for WL ambiguities.
       Vector<double> ambCombVec;
       Matrix<double> ambCombCov;

       ambCombVec = mapUC2Comb * ambUCVec;
       ambCombCov = mapUC2Comb * ambUCCov * transpose(mapUC2Comb);

       if(detail)
       {
         cout << "ambCombVec" << endl;
         cout <<  ambCombVec << endl;

         cout << "ambCombCov" << endl;
         cout <<  ambCombCov << endl;
       }

       i = 0;
       for(auto amb: combAmbSet)
       {
           combAmbData[amb] = ambCombVec(i);
           int j=0;
           for(auto amb2: combAmbSet)
           {
               combAmbCovData[amb][amb2] = ambCombCov(i,j);
               j++;
           }
           i++;
       }

   };

   void SolverPPPUCAR::getL1Ambs( Vector<double>& state,
                                Matrix<double>& covMatrix,
                                VariableSet& BL1Set,
                                VariableDataMap& BL1Data, 
                                CovarianceDataMap& BL1CovData )
   {
       for(auto amb: BL1Set)
       {
           BL1Data[amb] = state(amb.getNowIndex());
           for(auto amb2: BL1Set)
           {
               BL1CovData[amb][amb2] = covMatrix(amb.getNowIndex(),amb2.getNowIndex());
           }
       }
   };

   int SolverPPPUCAR::updateSols( VariableDataMap& fixedAmbData, 
                                Vector<double>& state,
                                Matrix<double>& covMatrix,
                                Vector<double>& stateFixed, 
                                Matrix<double>& covMatrixFixed )
   {

       Matrix<double> wMatrix = inverseChol(covMatrix);

       int numFixed = fixedAmbData.size();
       int numUnks = currentUnkSet.size();

       int numAll = numFixed + numUnks;
       Matrix<double> augGeometry(numAll, numUnks, 0.0);
       Vector<double> augPrefit(numAll, 0.0);
       Matrix<double> augWeight(numAll, numAll, 0.0);

       for(int i=0; i<numUnks; i++)
       {
           augGeometry(i,i) = 1.0;
           augPrefit(i) = state(i);
       }

       for(int i=0; i<numUnks; i++)
       {
           for(int j=0; j<numUnks; j++)
           {
               augWeight(i,j) = wMatrix(i,j);
           }
       }

       // to be continue;
       int i = numUnks;
       for(auto ad: fixedAmbData)
       {
           int j = ad.first.getNowIndex();
           augGeometry(i,j) = 1.0;
           augPrefit(i) = ad.second;
           i++;
       }

       // to be continue;
       for(int i=numUnks; i<numAll; i++)
       {
           augWeight(i,i) = 1.0E+9;
       }

       // now, let's update new solution
       Matrix<double> AT;
       AT = transpose(augGeometry);
        
       Matrix<double> tempCovMatrix = AT*augWeight*augGeometry;

       try {
           tempCovMatrix = inverseChol( tempCovMatrix );
       }
       catch(...)
       {
           InvalidSolver e("Unable to invert matrix tempCovMatrix");
           GPSTK_THROW(e);
       }

       Vector<double> tempState= tempCovMatrix * AT * augWeight * augPrefit; 

       stateFixed = tempState;
       covMatrixFixed = tempCovMatrix;

   };

  
   int SolverPPPUCAR::updateSolsForCombAmbs( VariableDataMap& fixedAmbData, 
                                             composedAmbsMap& composedAmbData,
                                             Vector<double>& state,
                                             Matrix<double>& covMatrix,
                                             Vector<double>& stateFixed, 
                                             Matrix<double>& covMatrixFixed )
   {
       int numCombFixed = fixedAmbData.size();
       int numUnks = currentUnkSet.size();

       // Let's get map matrix 
       Matrix<double> tempGeometry(numCombFixed, numUnks, 0.0);
       Vector<double> tempPrefit(numCombFixed, 0.0);
       Matrix<double> tempWeight(numCombFixed, numCombFixed, 0.0);

       int i=0;
       for(auto ad: fixedAmbData)
       {
           Variable amb1 = composedAmbData[ad.first].amb1; 
           Variable amb2 = composedAmbData[ad.first].amb2; 
           double coeff1 = composedAmbData[ad.first].coeff1;
           double coeff2 = composedAmbData[ad.first].coeff2;

           int j1 = amb1.getNowIndex();
           int j2 = amb2.getNowIndex();

           tempGeometry(i, j1) = coeff1;
           tempGeometry(i, j2) = coeff2;

           tempPrefit(i)       = ad.second;
           tempWeight(i,i)     = 1.0E+9;
           i++;
       }

       cout << "updateSolsForWLAmbs" << endl;
       cout << "tempGeometry" << endl;
       cout <<  tempGeometry  << endl;

       cout << "tempPrefit" << endl;
       cout <<  tempPrefit  << endl;

       cout << "tempWeight" << endl;
       cout <<  tempWeight  << endl;

       Vector<double> ucPrefit(numUnks, 0.0);
       Matrix<double> ucWeight(numUnks, numUnks, 0.0);

       ucPrefit = state;
       ucWeight = inverseChol(covMatrix);

       int numAll = numCombFixed + numUnks;
       Matrix<double> augGeometry(numAll, numUnks, 0.0);
       Vector<double> augPrefit(numAll, 0.0);
       Matrix<double> augWeight(numAll, numAll, 0.0);

       for(int i=0; i<numUnks; i++)
       {
           augGeometry(i,i) = 1.0;
           augPrefit(i) = ucPrefit(i);
       }

       for(int i=0; i<numUnks; i++)
       {
           for(int j=0; j<numUnks; j++)
           {
               augWeight(i,j) = ucWeight(i,j);
           }
       }

       // superpose the fixed ambiguity constraint equation
       for(int i=numUnks; i<numAll; i++)
       {
           augPrefit(i) = tempPrefit(i-numUnks);
           for(int j=0; j<numUnks;j++)
           {
               augGeometry(i,j) = tempGeometry(i-numUnks, j);
           }
       }

       // to be continue;
       // warning: only diagnal element is feeded.
       for(int i=numUnks; i<numAll; i++)
       {
           augWeight(i,i) = tempWeight(i-numUnks, i-numUnks);
       }

       cout <<"augGeometry"<< endl; 
       cout << augGeometry << endl; 

       cout <<"augWeight"<< endl; 
       cout << augWeight<< endl; 

       cout <<"augPrefit"<< endl; 
       cout << augPrefit<< endl; 

       // now, let's update new solution
       Matrix<double> AT = transpose(augGeometry);
        
       Matrix<double> tempCovMatrix = AT*augWeight*augGeometry;

       try {
           tempCovMatrix = inverseChol( tempCovMatrix );
       }
       catch(...)
       {
           InvalidSolver e("Unable to invert matrix tempCovMatrix");
           GPSTK_THROW(e);
       }

       Vector<double> tempState= tempCovMatrix * AT * augWeight * augPrefit; 

       stateFixed = tempState;
       covMatrixFixed = tempCovMatrix;

   };
  

   Variable SolverPPPUCAR::getVariable( const TypeID& type, 
                                      const VariableSet& varSet) const
      noexcept(false)
   {

         // Declare an iterator for 'stateMap' and go to the first element
      VariableSet::const_iterator it = varSet.begin();

         // Look for a variable with the same type
      while( (*it).getType() != type &&
             it != varSet.end() )
      {
         ++it;

         // If the same type is not found, throw an exception
         if( it == varSet.end() )
         {
             InvalidRequest e("Type not found in varSet.");
             GPSTK_THROW(e);
         }
      }

         // Else, return the corresponding value
      return (*it);

   }  // End of method 'SolverGeneral::getSolution()'


      /* Returns the solution associated to a given TypeID.
       *
       * @param type    TypeID of the solution we are looking for.
       *
       * \warning In the case the solution contains more than one variable
       * of this type, only the first one will be returned.
       */
   double SolverPPPUCAR::getSolution( const TypeID& type, 
                                  const Vector<double>& stateVec ) const
      noexcept(false)
   {
         // Declare an varIterator for 'stateMap' and go to the first element
      auto varIt = currentUnkSet.begin();
      while( (*varIt).getType() != type )
      {
         // If the same type is not found, throw an exception
         if( varIt == currentUnkSet.end() )
         {
             InvalidRequest e("Type not found in solution vector.");
             cerr << e << endl;
             exit(-1);
         }

         varIt++;
      }

         // Else, return the corresponding value
      return stateVec( (*varIt).getNowIndex() );

   }  // End of method 'SolverGeneral::getSolution()'

   bool SolverPPPUCAR::extractAmbData(Vector<double>& solution, 
                                    Matrix<double>& covMatrix,
                                    TypeID type,
                                    SatelliteSystem::Systems sys,
                                    VariableDataMap& ambMap,
                                    CovarianceDataMap& ambCovMap)
   {
       VariableSet ambSet;
       for(auto v: currentUnkSet)
       {
           SatelliteSystem::Systems currentSys = v.getSatSys();
           TypeID currentType = v.getType();
           if( currentSys == sys && currentType == type)
           {
               ambSet.insert(v);
           }
       }

       bool hasAmb(false);
       if(ambSet.size() > 0)
       {
           hasAmb = true;
       }

       for(auto amb: ambSet)
       {
           int i = amb.getNowIndex();
           ambMap[amb] = solution(i);
           ambCovMap[amb][amb] = covMatrix(i,i);
       }

       return hasAmb;

   };


   SatIDSet SolverPPPUCAR::extractSatSet(TypeID type, SatelliteSystem::Systems system)
   {
       SatIDSet satSet;
       for(auto v: currentUnkSet)
       {
           SatelliteSystem::Systems currentSys = v.getSatSys();
           TypeID currentType = v.getType();
           if(currentSys==system && currentType == type)
           {
               satSet.insert(v.getSatellite());
           }
       }
       return satSet;
   }

   VariableSet SolverPPPUCAR::extractAmbSet(TypeID type, SatelliteSystem::Systems system)
   {
       VariableSet varSet;
       for(auto v: currentUnkSet)
       {
           SatelliteSystem::Systems currentSys = v.getSatSys();
           TypeID currentType = v.getType();
           if(currentSys==system && currentType == type)
           {
               varSet.insert(v);
           }
       }
       return varSet;
   }


   // print float solutions
   void SolverPPPUCAR::printSols( std::ofstream* outStr,
                                  const CommonTime& epoch, 
                                  const Vector<double>& state,
                                  const Vector<double>& stateFixed, 
                                  int numSat,
                                  int numFixed,
                                  double ratio)
   {
       if(outStr==NULL)
       {
           cerr  << "you must feed printSols with output stream!" << endl;
           exit(-1);
       }

       *outStr << YDSTime(epoch).printf("%Y %03j %14.6s") << " ";

       // initial position
       Triple initialPos = nominalPosData[epoch];

       if(debug)
           cout << setprecision(3) << setw(14) << initialPos << endl;

       if(useNEU )
       {

           double dLat, dLon, dH, dt;

           // solution1
           dLat = getSolution(TypeID::dStaLat, state);
           dLon = getSolution(TypeID::dStaLon, state);
           dH   = getSolution(TypeID::dStaH,   state);
           dt   = getSolution(TypeID::dcdtSta, state);

           if(debug)
           cout 
               << dLat << " "
               << dLon << " "
               << dH << " "
               << endl;

           Triple neuVec;
           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;

           Triple xyzVec;

           if(debug)
           {
               cout << "neu" << endl;
               cout << neuVec << endl;
           }

           // now, let's convert dLat, dLon and dH to
           NEUUtil neuUtil(initialPos);
           xyzVec = neuUtil.convertToXYZ(neuVec);

           if(debug)
           {
               cout << "xyzVec" << endl;
               cout << xyzVec << endl;
           }

           Triple estPos = initialPos + xyzVec;

           if(debug)
               cout << "estPos:" << estPos << endl;

           *outStr << rightJustify(asString(estPos[0],3),   12 )  << " "
                   << rightJustify(asString(estPos[1],3),   12 )  << " "
                   << rightJustify(asString(estPos[2],3),   12 )  << " "
                   << rightJustify(asString(dt  ,3),        9 )  << " "
                   << rightJustify(asString<int>(numSat),4) <<  " "
                   ;

           *outStr << rightJustify(asString(dLat,3), 6 )  << " "
                   << rightJustify(asString(dLon,3), 6 )  << " "
                   << rightJustify(asString(dH  ,3), 6 )  << " "
                   ;
           // solution2
           dLat = getSolution(TypeID::dStaLat, stateFixed);
           dLon = getSolution(TypeID::dStaLon, stateFixed);
           dH   = getSolution(TypeID::dStaH,   stateFixed);
           dt   = getSolution(TypeID::dcdtSta,   stateFixed);

           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;
           xyzVec = neuUtil.convertToXYZ(neuVec);
           estPos = initialPos + xyzVec;

           *outStr << rightJustify(asString(estPos[0],3),  12)  << " "
                   << rightJustify(asString(estPos[1],3),  12)  << " "
                   << rightJustify(asString(estPos[2],3),  12)  << " "
                   << rightJustify(asString(dt  ,3),       9)  << " "
                   << rightJustify(asString<int>(numFixed),4)  << " "
                   << rightJustify(asString(ratio,1),      4)  << " "
                   ;

           *outStr << rightJustify(asString(dLat,3), 6 )  << " "
                   << rightJustify(asString(dLon,3), 6 )  << " "
                   << rightJustify(asString(dH  ,3), 6 )  << " "
                   ;
           
           *outStr << endl;

       }
       else
       {
           double dx, dy, dz, dt;
           dx = getSolution(TypeID::dStaX,   state);
           dy = getSolution(TypeID::dStaY,   state);
           dz = getSolution(TypeID::dStaZ,   state);
           dt = getSolution(TypeID::dcdtSta, state);

           Triple xyzVec;
           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           Triple estPos = initialPos + xyzVec;

              // print time

           *outStr << rightJustify(asString(estPos[0],3), 12)<< " "
                   << rightJustify(asString(estPos[1],3), 12)<< " "
                   << rightJustify(asString(estPos[2],3), 12)<< " " 
                   << rightJustify(asString(dt,3),         9)<< " "
                   << rightJustify(asString<int>(numSat),4) <<  " "
                   ;

           *outStr << rightJustify(asString(dx,3),  6)<< " "
                   << rightJustify(asString(dy,3),  6)<< " "
                   << rightJustify(asString(dz,3),  6)<< " " 
                   ;

           dx = getSolution(TypeID::dStaX,   stateFixed);
           dy = getSolution(TypeID::dStaY,   stateFixed);
           dz = getSolution(TypeID::dStaZ,   stateFixed);
           dt = getSolution(TypeID::dcdtSta, stateFixed);

           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           if(debug)
           {
               cout << "xyzVec" << endl;
               cout << xyzVec << endl;
           }

           estPos = initialPos + xyzVec;

           if(debug)
               cout << "estPos:" << fixed  << setprecision(3) << setw(14) << estPos << endl;

           *outStr << rightJustify(asString(estPos[0],3),  12)<< " "
                   << rightJustify(asString(estPos[1],3),  12)<< " "
                   << rightJustify(asString(estPos[2],3),  12)<< " " 
                   << rightJustify(asString(dt,3),          9)<< " "
                   << rightJustify(asString<int>(numFixed), 3)<< " "
                   << rightJustify(asString(ratio,1),       4)  << " "
                   ;

           *outStr << rightJustify(asString(dx,3),  6)<< " "
                   << rightJustify(asString(dy,3),  6)<< " "
                   << rightJustify(asString(dz,3),  6)<< " " 
                   ;
           
           *outStr << endl;
       }
   }


     // print float solutions
     void SolverPPPUCAR::printAmbs( std::ofstream* outStr,
                                  const CommonTime& epoch,
                                  Vector<double>& solution,
                                  Matrix<double>& covMatrix)
     {
         if(outStr==NULL)
         {
             cerr  << "you must feed printAmbs with output stream!" << endl;
             exit(-1);
         }

         VariableSet ambSet;
         for(auto v: currentUnkSet)
         {
             if( v.getArcIndexed() )
             {
                 ambSet.insert(v);
             }
         }

         for(auto amb: ambSet)
         {
             SourceID source = amb.getSource();
             SatID sat       = amb.getSatellite();
             double satArc   = amb.getArc();
             TypeID type     = amb.getType();

             double value    = solution(amb.getNowIndex());
             double variance = covMatrix(amb.getNowIndex(), amb.getNowIndex());

             double frac     = value - std::floor(value+0.5);

                // print time
             *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
             *outStr << source << " "
                     << sat << " "
                     << rightJustify(asString<int>(int(satArc)),6) << " "
                     << type << " "
                     << rightJustify(asString(value,6),13) << " "
                     << rightJustify(asString(variance,6),13) << " "
                     << rightJustify(asString(frac,6),13) << " "
                     << endl;
         }

     };

     // print mw values
     void SolverPPPUCAR::printLog( std::ofstream* outStr, 
                               const CommonTime& epoch, 
                               systemTypeValueMap systemTypeBiasData,
                               int fixNum)
     {
                // print time
         for(auto stb: systemTypeBiasData)
         {
             SatelliteSystem system(stb.first);
             typeValueMap typeBiasData = stb.second;

             for(auto tb: typeBiasData)
             {
                *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
                *outStr << system << " "
                        << tb.first << " "
                        << rightJustify(asString(tb.second,6),10) << " " 
                        << rightJustify(asString<int>(fixNum),10) << " " 
                        << endl;
             }
         }
     }

}  // End of namespace gpstk
